// term.c

#include "term.h"
#include "Inverse_kinematics.h"
#include "forward_kinematics.h"
#include "math.h"
#include "motor.h"
#include "mqtt_comm.h"
#include "myprint.h"
#include "rs485.h"
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

// +++ 新增：用于跟踪“理想”或“指令”末端绝对位姿的静态变量 +++
// 这就是我们的“理想目标账本”
static double g_commanded_absolute_pose[6] = {0.0};
/* 全局开关：1=允许做光栅检查，0=不做 */
static volatile int grating_check_enable = 0;

// 在 judge_task 函数外部或函数内部定义一个静态变量
static int32_t g_locked_ideal_grating_value = 0;

/* 光栅精确校正相关变量 */
static GratingCheckState grating_check_state = GRATING_CHECK_IDLE;
static int micro_adjustment_counter = 0;
static const int MAX_MICRO_ADJUSTMENTS = 10; // 最大微调次数，防止无限循环
static const int GRATING_TOLERANCE = 5; // 光栅偏差容忍范围 ±5

/* 添加光栅到脉冲转换比例常量（从motor.c中复制） */
static const double GRATING_TO_PULSE_RATIO = 4096.0 / 10735.0;

/* 各轴零点脉冲数 */
extern int ZERO_PULSE_THETA6; // 旋转关节6
// static const int ZERO_PULSE_THETA6 = 0;     // 旋转关节6
static const int ZERO_PULSE_THETA5 = 71158;  // 旋转关节5
static const int ZERO_PULSE_D4 = 795823;     // 位移关节4
static const int ZERO_PULSE_D3 = 703125;     // 位移关节3
static const int ZERO_PULSE_D2 = 705678;     // 位移关节2
static const int ZERO_PULSE_THETA1 = 226148; // 旋转关节1
/* 电机脉冲限位 (顺序：theta6, theta5, d4, d3, d2, theta1) */
// static const int LIMIT_THETA6_MIN = -11500; // theta6限位范围需要调整
// static const int LIMIT_THETA6_MAX = 26000;

extern int LIMIT_THETA6_MIN;
extern int LIMIT_THETA6_MAX;

static const int LIMIT_THETA5_MIN = -18000;
static const int LIMIT_THETA5_MAX = 160000;

static const int LIMIT_D4_MIN = 140000;
static const int LIMIT_D4_MAX = 1560000;

static const int LIMIT_D3_MIN = 30000;
static const int LIMIT_D3_MAX = 1370000;

static const int LIMIT_D2_MIN = 30000;
static const int LIMIT_D2_MAX = 1370000;

static const int LIMIT_THETA1_MIN = 22853;
static const int LIMIT_THETA1_MAX = 261784;

/* 转换比例 */
static const double POSITION_PULSE_PER_M = 1.0 / 50e-9; // 位移：1脉冲对应50nm
static const double ROTATIONAL_PULSE_PER_DEG_DD =
    1.0 / 0.001; // 力矩电机：1脉冲对应0.001°
static const double ROTATIONAL_PULSE_PER_DEG_ZOLIX =
    1.0 / 0.0002475921753; // 天瀚精密电机：1脉冲对应0.0002475921753

static const uint32_t GRATING_TARGET_VALUE = 103051500; // 理想光栅值
static const double GRATING_TO_ANGLE_RATIO =
    1.01413755 / 10735.0; //光栅尺到角度的转换比例

// 全局控制命令
MotorSet cmdSet = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                   MODEL_CSP,          MODEL_CSP,          {0, 0, 0, 0, 0, 0}};
pthread_mutex_t cmdSet_mutex = PTHREAD_MUTEX_INITIALIZER;
// mqtt数据
extern RobotControlData g_robotControlData;
extern RobotStateData g_robotStateData;
// 模式切换时更新状态
// void updateStatues(MotorSet *cmd) {
//   if (cmd->mode != cmd->previous_mode) {
//     cmd->state[0] = 0;
//     cmd->previous_mode = cmd->mode; // 更新 previous_mode
//   }
// }

void updateStatues(MotorSet *cmd) {
  if (cmd->mode != cmd->previous_mode) {
    // 使用循环重置所有电机的状态
    for (int i = 0; i < 6; i++) { // 或者使用 MOTOR_NUM 宏
      cmd->state[i] = 0;
    }
    cmd->previous_mode = cmd->mode;
  }
}

/* 光栅开启/关闭 */
void enable_grating_check(void) { grating_check_enable = 1; }
void disable_grating_check(void) { grating_check_enable = 0; }

//正运动学转换 - 支持增量控制
int posconvert(const RobotControlData *recv, int *outpos) {
  int ret = 0;
  int temp;

  // 逆运动学求解：末端位姿 → 关节脉冲
  if (recv->taskType == 2) {
    // // ====== 绝对位置控制（原有逻辑） ======
    // // recv->AbsolutePose:
    // // DeviceControl话题的parameter，传给inverse_kinematics的parameter参数
    // // recv->Delta:
    // // DeviceControl话题的obj_pos，传给inverse_kinematics的x,y,z,rx,ry,rz参数
    // inverse_kinematics(recv->AbsolutePose, // parameter参数
    //                    recv->Delta[0],     // x_um
    //                    recv->Delta[1],     // y_um
    //                    recv->Delta[2],     // z_um
    //                    recv->Delta[3],     // rx_deg
    //                    recv->Delta[4],     // ry_deg
    //                    recv->Delta[5],     // rz_deg
    //                    &outpos[0], &outpos[1], &outpos[2], &outpos[3],
    //                    &outpos[4], &outpos[5]);
    // 直接用收到的绝对位置，覆盖我们的“理想目标账本”
    memcpy(g_commanded_absolute_pose, recv->AbsolutePose,
           sizeof(g_commanded_absolute_pose));

  } else if (recv->taskType == 6) {
    // 在“理想目标账本”的当前值上，累加收到的增量
    for (int i = 0; i < 6; i++) {
      g_commanded_absolute_pose[i] += recv->AbsolutePose[i];
    }
    // // ====== 增量位置控制（新增逻辑） ======
    // // recv->AbsolutePose: 增量值
    // // recv->Delta: obj_pos变换参数

    // // 构建当前绝对位置参数（从g_robotStateData获取）
    // double current_parameter[6];
    // current_parameter[0] = g_robotStateData.PositionXYZ[0]; // x (μm)
    // current_parameter[1] = g_robotStateData.PositionXYZ[1]; // y (μm)
    // current_parameter[2] = g_robotStateData.PositionXYZ[2]; // z (μm)
    // current_parameter[3] = g_robotStateData.PositionXYZ[3]; // Rx (°)
    // current_parameter[4] = g_robotStateData.PositionXYZ[4]; // Ry (°)
    // current_parameter[5] = g_robotStateData.PositionXYZ[5]; // Rz (°)

    // // 调用增量式逆运动学
    // incremental_inverse_kinematics(recv->AbsolutePose, // 增量参数
    //                                current_parameter,  // 当前绝对位置
    //                                recv->Delta[0],     // x_um (obj_pos)
    //                                recv->Delta[1],     // y_um
    //                                recv->Delta[2],     // z_um
    //                                recv->Delta[3],     // rx_deg
    //                                recv->Delta[4],     // ry_deg
    //                                recv->Delta[5],     // rz_deg
    //                                &outpos[0], &outpos[1], &outpos[2],
    //                                &outpos[3], &outpos[4], &outpos[5]);

  } else if (recv->taskType == 1) {
    // ====== 关节控制（保持原有逻辑） ======
    // 位移轴（index 1..3）以 微米(μm) 为单位传输
    // 旋转轴（index 0,4,5）以 度(deg) 为单位传输

    // rz (theta1) — recv->Delta[0] 是度
    outpos[0] = (int)llround(recv->Delta[0] * ROTATIONAL_PULSE_PER_DEG_DD) +
                ZERO_PULSE_THETA1;

    // d2 (y) — recv->Delta[1] 是 μm -> 转为 m 再转脉冲
    double d2_m = recv->Delta[1] / 1e6; // μm -> m
    outpos[1] = (int)llround(d2_m * POSITION_PULSE_PER_M) + ZERO_PULSE_D2;

    // d3 (x)
    double d3_m = recv->Delta[2] / 1e6;
    outpos[2] = (int)llround(d3_m * POSITION_PULSE_PER_M) + ZERO_PULSE_D3;

    // d4 (z)
    double d4_m = recv->Delta[3] / 1e6;
    outpos[3] = (int)llround(d4_m * POSITION_PULSE_PER_M) + ZERO_PULSE_D4;

    // rx (theta5) — 度 -> 脉冲
    outpos[4] = (int)llround(recv->Delta[4] * ROTATIONAL_PULSE_PER_DEG_DD) +
                ZERO_PULSE_THETA5;

    // ry (theta6) — 度 -> 脉冲
    outpos[5] = (int)llround(recv->Delta[5] * ROTATIONAL_PULSE_PER_DEG_ZOLIX) +
                ZERO_PULSE_THETA6;
    goto axis_conversion_done;
  } else if (recv->taskType == 7) {
    // ====== 相对关节控制（新增逻辑） ======
    // recv->Delta 数组包含各轴的增量值 (度 或 微米)
    // 目标脉冲 = 当前规划脉冲 + 增量脉冲

    // rz (theta1) — recv->Delta[0] 是度
    double delta_pulse_theta1 = recv->Delta[0] * ROTATIONAL_PULSE_PER_DEG_DD;
    outpos[0] = cmdSet.plan_pos[5] + (int)llround(delta_pulse_theta1);

    // d2 (y) — recv->Delta[1] 是 μm
    double d2_m_delta = recv->Delta[1] / 1e6; // μm -> m
    double delta_pulse_d2 = d2_m_delta * POSITION_PULSE_PER_M;
    outpos[1] = cmdSet.plan_pos[4] + (int)llround(delta_pulse_d2);

    // d3 (x) — recv->Delta[2] 是 μm
    double d3_m_delta = recv->Delta[2] / 1e6;
    double delta_pulse_d3 = d3_m_delta * POSITION_PULSE_PER_M;
    outpos[2] = cmdSet.plan_pos[3] + (int)llround(delta_pulse_d3);

    // d4 (z) — recv->Delta[3] 是 μm
    double d4_m_delta = recv->Delta[3] / 1e6;
    double delta_pulse_d4 = d4_m_delta * POSITION_PULSE_PER_M;
    outpos[3] = cmdSet.plan_pos[2] + (int)llround(delta_pulse_d4);

    // rx (theta5) — recv->Delta[4] 是度
    double delta_pulse_theta5 = recv->Delta[4] * ROTATIONAL_PULSE_PER_DEG_DD;
    outpos[4] = cmdSet.plan_pos[1] + (int)llround(delta_pulse_theta5);

    // ry (theta6) — recv->Delta[5] 是度
    double delta_pulse_theta6 = recv->Delta[5] * ROTATIONAL_PULSE_PER_DEG_ZOLIX;
    outpos[5] = cmdSet.plan_pos[0] + (int)llround(delta_pulse_theta6);
    goto axis_conversion_done;
  } else {
    // 其他模式（如回零）保持原有逻辑
    return ret;
  }
  // --- 统一的逆运动学计算 ---
  // 对于所有末端控制，我们都使用“理想目标账本”作为唯一的目标
  inverse_kinematics(g_commanded_absolute_pose, // <-- 关键！使用理想目标
                     recv->Delta[0],            // obj_pos
                     recv->Delta[1], recv->Delta[2], recv->Delta[3],
                     recv->Delta[4], recv->Delta[5], &outpos[0], &outpos[1],
                     &outpos[2], &outpos[3], &outpos[4], &outpos[5]);

axis_conversion_done: // 标签，用于跳过IK计算

  /************* 交换顺序 *************/
  for (int i = 0; i < 3; i++) {
    temp = outpos[i];
    outpos[i] = outpos[5 - i];
    outpos[5 - i] = temp;
  }

  /************* 限位判断 *************/
  if (outpos[0] < LIMIT_THETA6_MIN || outpos[0] > LIMIT_THETA6_MAX) {
    ret = 1;
  } else if (outpos[1] < LIMIT_THETA5_MIN || outpos[1] > LIMIT_THETA5_MAX) {
    ret = 1;
  } else if (outpos[2] < LIMIT_D4_MIN || outpos[2] > LIMIT_D4_MAX) {
    ret = 1;
  } else if (outpos[3] < LIMIT_D3_MIN || outpos[3] > LIMIT_D3_MAX) {
    ret = 1;
  } else if (outpos[4] < LIMIT_D2_MIN || outpos[4] > LIMIT_D2_MAX) {
    ret = 1;
  } else if (outpos[5] < LIMIT_THETA1_MIN || outpos[5] > LIMIT_THETA1_MAX) {
    ret = 1;
  }
  return ret;
}

// 根据状态设置电机
int SetHome(int *target_pos) {
  target_pos[0] = ZERO_PULSE_THETA6;
  target_pos[1] = ZERO_PULSE_THETA5;
  target_pos[2] = ZERO_PULSE_D4;
  target_pos[3] = ZERO_PULSE_D3;
  target_pos[4] = ZERO_PULSE_D2;
  target_pos[5] = ZERO_PULSE_THETA1;

  return *target_pos;
}

// 斜坡函数平滑控制
int updateSetPos(int setpos, int target_pos, float max_speed) {
  int diff = target_pos - setpos; // 计算目标与当前位置的差
  // 如果差距小于步长，直接到达目标
  if (abs(diff) <= max_speed)
    return target_pos;

  // 向目标方向移动一个步长
  if (diff > 0)
    return setpos + max_speed;
  else
    return setpos - max_speed;
}

int SetMotor(RobotControlData *recv) {
  int ret = 0;

  if (recv->taskType == 5) {
    // 暂停模式：不改变目标位置，保持当前状态
    return 0;

  } else if (recv->taskType == 3) {
    SetHome(cmdSet.target_pos); // 设置目标位置为零点
    cmdSet.mode = MODEL_CSP;

  } else if (recv->taskType == 2 || recv->taskType == 6 ||
             recv->taskType == 1 || recv->taskType == 7) {
    // 末端位置模式（绝对或增量）和关节控制模式(绝对或相对)
    ret = posconvert(recv, cmdSet.target_pos); // 逆运动学求解
    cmdSet.mode = MODEL_CSP;
  }

  return ret;
}

void driveMotor(MotorSet *cmd) {
  // cmdSet.plan_pos[0] = updateSetPos(cmdSet.plan_pos[0], cmdSet.target_pos[0],
  // 22); // 电机速度 3度/s cmdSet.plan_pos[1] =
  // updateSetPos(cmdSet.plan_pos[1], cmdSet.target_pos[1], 12); // 电机速度
  // 3度/s cmdSet.plan_pos[2] = updateSetPos(cmdSet.plan_pos[2],
  // cmdSet.target_pos[2], 400);// 电机速度 5mm/s cmdSet.plan_pos[3] =
  // updateSetPos(cmdSet.plan_pos[3], cmdSet.target_pos[3], 400);// 电机速度
  // 5mm/s cmdSet.plan_pos[4] = updateSetPos(cmdSet.plan_pos[4],
  // cmdSet.target_pos[4], 400);// 电机速度 5mm/s cmdSet.plan_pos[5] =
  // updateSetPos(cmdSet.plan_pos[5], cmdSet.target_pos[5], 12); // 电机速度
  // 3度/s
  if (g_robotControlData.step > 50) {
    g_robotControlData.step = 50;
  } else if (g_robotControlData.step < 5) {
    g_robotControlData.step = 5;
  }
  cmdSet.plan_pos[0] =
      updateSetPos(cmdSet.plan_pos[0], cmdSet.target_pos[0],
                   2 * 0.1f * g_robotControlData.step); // 电机速度 3度/s
  cmdSet.plan_pos[1] =
      updateSetPos(cmdSet.plan_pos[1], cmdSet.target_pos[1],
                   1 * 0.1f * g_robotControlData.step); // 电机速度 3度/s
  cmdSet.plan_pos[2] =
      updateSetPos(cmdSet.plan_pos[2], cmdSet.target_pos[2],
                   40 * 0.1f * g_robotControlData.step); // 电机速度 5mm/s
  cmdSet.plan_pos[3] =
      updateSetPos(cmdSet.plan_pos[3], cmdSet.target_pos[3],
                   40 * 0.1f * g_robotControlData.step); // 电机速度 5mm/s
  cmdSet.plan_pos[4] =
      updateSetPos(cmdSet.plan_pos[4], cmdSet.target_pos[4],
                   40 * 0.1f * g_robotControlData.step); // 电机速度 5mm/s
  cmdSet.plan_pos[5] =
      updateSetPos(cmdSet.plan_pos[5], cmdSet.target_pos[5],
                   2 * 0.1f * g_robotControlData.step); // 电机速度 3度/s
}

// 新增：执行光栅微调的函数
void perform_grating_micro_adjustment(int32_t ideal_grating_diff) {
  micro_adjustment_counter++;

  // 计算需要的电机脉冲补偿
  // 注意：这里假设光栅偏差直接对应theta6电机的调整
  int32_t micro_pulse_adjustment =
      (int32_t)(ideal_grating_diff * GRATING_TO_PULSE_RATIO);

  printf("执行第%d次微调,光栅偏差: %d, 电机补偿脉冲: %d\n",
         micro_adjustment_counter, ideal_grating_diff, micro_pulse_adjustment);

  // 更新theta6电机的目标位置（索引0对应theta6）
  pthread_mutex_lock(&cmdSet_mutex);
  cmdSet.target_pos[0] =
      cmdSet.target_pos[0] + micro_pulse_adjustment; // 注意符号方向

  // 检查调整后的位置是否在限位范围内
  if (cmdSet.target_pos[0] < LIMIT_THETA6_MIN ||
      cmdSet.target_pos[0] > LIMIT_THETA6_MAX) {
    printf("警告：微调后位置超出限位范围，恢复原位置\n");
    cmdSet.target_pos[0] =
        cmdSet.target_pos[0] - micro_pulse_adjustment; // 恢复原位置
    grating_check_state = GRATING_CHECK_COMPLETE;      // 停止调整
  } else {
    grating_check_state = GRATING_CHECK_ADJUSTING; // 设置为调整状态
    printf("theta6目标位置更新为: %d\n", cmdSet.target_pos[0]);
  }
  pthread_mutex_unlock(&cmdSet_mutex);
}

// 新增：重置光栅校正状态的函数（在接收新任务时调用）
void reset_grating_check_state(void) {
  grating_check_state = GRATING_CHECK_IDLE;
  micro_adjustment_counter = 0;
  disable_grating_check();
  g_locked_ideal_grating_value = 0; // 重置
  printf("光栅校正状态已重置\n");
}

// 修改后的 judge_task 函数
void judge_task(RobotStateData *state) {
  // 检查是否已到达目标位置
  if (cmdSet.mode == MODEL_CSP && (memcmp(cmdSet.plan_pos, cmdSet.target_pos,
                                          sizeof(cmdSet.plan_pos)) == 0)) {

    // 根据光栅校正状态决定下一步操作
    switch (grating_check_state) {
    case GRATING_CHECK_IDLE:
      // 首次到达目标位置，开始光栅检查
      if (grating_init_done) {
        grating_check_state = GRATING_CHECK_READING;
        enable_grating_check();

        // ==================== 核心修正点 开始 ====================
        // 在进入精调的第一次，计算并锁定本次任务的唯一目标
        double target_angle = 0.0;
        if (g_robotControlData.taskType == 2 ||
            g_robotControlData.taskType == 6 ||
            g_robotControlData.taskType == 7) { // 末端控制
          target_angle = (cmdSet.target_pos[0] - ZERO_PULSE_THETA6) /
                         ROTATIONAL_PULSE_PER_DEG_ZOLIX;
        } else if (g_robotControlData.taskType == 1) { // 轴控制
          target_angle = g_robotControlData.Delta[5];
        }

        g_locked_ideal_grating_value =
            (int32_t)llround((double)GRATING_TARGET_VALUE -
                             target_angle / GRATING_TO_ANGLE_RATIO);

        printf("精调目标已锁定: %d\n", g_locked_ideal_grating_value);
        // ==================== 核心修正点 结束 ====================

        printf("开始光栅精确校正检查...\n");
      } else {
        // 光栅未初始化，直接完成任务
        state->taskState = 1;
        printf("光栅未初始化，任务完成\n");
      }
      break;

    case GRATING_CHECK_READING:
      // 光栅读取完成后会在下面处理
      break;

    case GRATING_CHECK_ADJUSTING:
      // 微调完成，重新检查
      grating_check_state = GRATING_CHECK_READING;
      enable_grating_check();
      printf("微调完成，重新检查光栅偏差...\n");
      break;

    case GRATING_CHECK_COMPLETE:
      // 光栅校正完成，任务结束
      state->taskState = 1;
      printf("光栅精确校正完成，任务结束\n");
      break;
    }
  }

  // 处理光栅读取
  if (grating_check_enable && grating_check_state == GRATING_CHECK_READING) {
    if (grating_init_done) {
      uint32_t current_grating_value;
      if (grating_scale_read_current_value(&current_grating_value) == 0) {
        // ==================== 核心修正点 开始 ====================

        // 1. 【新增】根据当前光栅值，正确计算“实际物理角度”
        //    这个计算的基准是固定的物理零点 GRATING_TARGET_VALUE
        int32_t physical_grating_diff =
            (int32_t)current_grating_value - (int32_t)GRATING_TARGET_VALUE;
        actual_theta6_angle =
            -(double)physical_grating_diff * GRATING_TO_ANGLE_RATIO;

        // 2. 【保持】根据锁定的目标，计算用于控制的“理想偏差”
        int32_t ideal_grating_diff =
            (int32_t)current_grating_value - g_locked_ideal_grating_value;

        // 3. 【修正】在 printf 中使用正确的变量 actual_theta6_angle
        printf("光栅读数: %u, 理想偏差: %d (目标: %d), 实际角度: %.6f\n",
               current_grating_value, ideal_grating_diff,
               g_locked_ideal_grating_value,
               actual_theta6_angle); // <--- 确保这里是 actual_theta6_angle

        // ==================== 核心修正点 结束 ====================

        // 检查偏差是否在容忍范围内
        if (abs(ideal_grating_diff) <= GRATING_TOLERANCE) {
          // 偏差在容忍范围内，校正完成
          grating_check_state = GRATING_CHECK_COMPLETE;
          printf("光栅偏差在容忍范围内(%d <= %d)，校正完成\n",
                 abs(ideal_grating_diff), GRATING_TOLERANCE);
        } else if (micro_adjustment_counter >= MAX_MICRO_ADJUSTMENTS) {
          // 达到最大调整次数，停止调整
          grating_check_state = GRATING_CHECK_COMPLETE;
          printf("达到最大微调次数(%d)，停止调整。最终偏差: %d\n",
                 MAX_MICRO_ADJUSTMENTS, ideal_grating_diff);
        } else {
          // 需要进行微调
          perform_grating_micro_adjustment(ideal_grating_diff);
        }

        disable_grating_check();
      } else {
        printf("光栅读取失败，使用编码器值完成任务\n");
        grating_check_state = GRATING_CHECK_COMPLETE;
        disable_grating_check();
      }
    } else {
      printf("光栅尺未初始化，使用编码器值完成任务\n");
      grating_check_state = GRATING_CHECK_COMPLETE;
      disable_grating_check();
    }
  }
}

void *process_main(void *arg) {
  // 新增一个标志，确保只在启动时同步一次
  int is_first_run = 1;

  while (1) {
    // print_robot_control_data(&g_robotControlData);
    // printMotorSet(&cmdSet);
    // printf("taskState: %d\n", g_robotStateData.taskState);
    /********************* 接收任务 ********************/
    if (g_robotControlData.taskType == 4) {        // 急停模式
      g_robotStateData.taskState = 201;            // 任务急停
    } else if (g_robotControlData.taskType == 5) { // 暂停模式
      g_robotStateData.taskState = 5;              // 任务暂停状态
    }

    // 当电机初始化完成，可以接收任务时...
    if (g_robotStateData.taskState == 1 && is_first_run) {
      // 用机器人当前的实际末端位置，初始化我们的“理想目标账本”
      memcpy(g_commanded_absolute_pose, g_robotStateData.PositionXYZ,
             sizeof(g_commanded_absolute_pose));
      is_first_run = 0; // 关闭标志，确保不再同步
      printf("系统启动：理想目标位姿已与机器人初始位姿同步。\n");
    }

    if (g_robotStateData.taskState == 1 || g_robotStateData.taskState == 104 ||
        g_robotStateData.taskState == 204 ||
        g_robotStateData.taskState ==
            5) { // 任务执行完成可以接收任务、命令警告也可以重新获取任务
      if (g_robotControlData.taskNum != g_robotStateData.taskNum) { // 新任务
        // 任务接收
        if (g_robotControlData.taskType == 5) {
          // 暂停模式：保持当前位置，不执行新的运动
          g_robotStateData.taskState = 5; // 设置为暂停状态
          g_robotStateData.taskNum = g_robotControlData.taskNum;
          g_robotStateData.taskType = g_robotControlData.taskType;

          // 暂停时将目标位置设置为当前位置，停止运动
          pthread_mutex_lock(&cmdSet_mutex);
          memcpy(cmdSet.target_pos, cmdSet.plan_pos, sizeof(cmdSet.target_pos));
          pthread_mutex_unlock(&cmdSet_mutex);

        } else {
          // 正常任务处理
          g_robotStateData.taskState = 0; //开始执行新任务
          g_robotStateData.taskNum = g_robotControlData.taskNum; // 更新任务编号
          g_robotStateData.taskType =
              g_robotControlData.taskType; // 更新任务类型

          // 重置光栅校正状态
          reset_grating_check_state();

          pthread_mutex_lock(&cmdSet_mutex);
          int ret = SetMotor(&g_robotControlData); // 配置电机
          pthread_mutex_unlock(&cmdSet_mutex);
          if (ret != 0) {                     // 逆运动学求解超出范围
            g_robotStateData.taskState = 104; // 指令位置超过了量程
          }
        }
      }
    } else if (g_robotStateData.taskState == 0) { //任务执行中
      // 任务执行
      pthread_mutex_lock(&cmdSet_mutex);
      driveMotor(&cmdSet);
      pthread_mutex_unlock(&cmdSet_mutex);
      // 任务完成判断
      judge_task(&g_robotStateData);
    } else if (g_robotStateData.taskState == 201 ||
               g_robotStateData.taskState == 202) { // 任务急停
      break;                                        // 错误退出
    } else if (g_robotStateData.taskState == 5) {   // 暂停状态
      // 暂停状态下不执行运动，但保持电机使能
      // 可以在这里添加暂停状态的特殊处理逻辑

    } // 其他状态继续等待;
    usleep(4000);
  }
  return NULL;
}

int term_init() {
  pthread_t process_thread;
  pthread_create(&process_thread, NULL, process_main, NULL);
  pthread_detach(process_thread);
  return 0;
}
